

<!DOCTYPE html>
<!--[if IE 8]><html class="no-js lt-ie9" lang="en" > <![endif]-->
<!--[if gt IE 8]><!--> <html class="no-js" lang="en" > <!--<![endif]-->
<head>
  <meta charset="utf-8">
  
  <meta name="viewport" content="width=device-width, initial-scale=1.0">
  
  <title>2D Robot SLAM - Benchmark &mdash; Unscented Kalman Filtering on (Parallelizable) Manifolds alpha documentation</title>
  

  
  

  

  
  
    

  

  
  
    <link rel="stylesheet" href="../_static/css/theme.css" type="text/css" />
  

  
    <link rel="stylesheet" href="../_static/gallery.css" type="text/css" />
  
    <link rel="stylesheet" href="../_static/custom.css" type="text/css" />
  

  
        <link rel="index" title="Index"
              href="../genindex.html"/>
        <link rel="search" title="Search" href="../search.html"/>
    <link rel="top" title="Unscented Kalman Filtering on (Parallelizable) Manifolds alpha documentation" href="../index.html"/>
        <link rel="up" title="Benchmarks" href="../benchmarks.html"/>
        <link rel="next" title="Filters" href="../filter.html"/>
        <link rel="prev" title="Navigation on Flat Earth - Benchmark" href="inertial_navigation.html"/> 

  
  <script src="../_static/js/modernizr.min.js"></script>

</head>

<body class="wy-body-for-nav" role="document">

  <div class="wy-grid-for-nav">

    
    <nav data-toggle="wy-nav-shift" class="wy-nav-side">
      <div class="wy-side-scroll">
        <div class="wy-side-nav-search">
          

          
            <a href="../index.html" class="icon icon-home"> Unscented Kalman Filtering on (Parallelizable) Manifolds
          

          
            
            <img src="../_static/blacklogo.png" class="logo" />
          
          </a>

          
            
            
              <div class="version">
                1.0
              </div>
            
          

          
<div role="search">
  <form id="rtd-search-form" class="wy-form" action="../search.html" method="get">
    <input type="text" name="q" placeholder="Search docs" />
    <input type="hidden" name="check_keywords" value="yes" />
    <input type="hidden" name="area" value="default" />
  </form>
</div>

          
        </div>

        <div class="wy-menu wy-menu-vertical" data-spy="affix" role="navigation" aria-label="main navigation">
          
            
            
                <ul class="current">
<li class="toctree-l1"><a class="reference internal" href="../install.html">Installation</a></li>
<li class="toctree-l1"><a class="reference internal" href="../auto_examples/localization.html">Tutorial</a></li>
<li class="toctree-l1"><a class="reference internal" href="../examples.html">Examples</a></li>
<li class="toctree-l1 current"><a class="reference internal" href="../benchmarks.html">Benchmarks</a><ul class="current">
<li class="toctree-l2"><a class="reference internal" href="localization.html">2D Robot Localization</a></li>
<li class="toctree-l2"><a class="reference internal" href="attitude.html">Attitude Estimation with an IMU</a></li>
<li class="toctree-l2"><a class="reference internal" href="inertial_navigation.html">Navigation on Flat Earth</a></li>
<li class="toctree-l2 current"><a class="current reference internal" href="#">2D Robot SLAM</a><ul>
<li class="toctree-l3"><a class="reference internal" href="#import">Import</a></li>
<li class="toctree-l3"><a class="reference internal" href="#simulation-setting">Simulation Setting</a></li>
<li class="toctree-l3"><a class="reference internal" href="#filter-design">Filter Design</a></li>
<li class="toctree-l3"><a class="reference internal" href="#monte-carlo-runs">Monte-Carlo Runs</a><ul>
<li class="toctree-l4"><a class="reference internal" href="#results">Results</a></li>
</ul>
</li>
<li class="toctree-l3"><a class="reference internal" href="#conclusion">Conclusion</a></li>
</ul>
</li>
</ul>
</li>
<li class="toctree-l1"><a class="reference internal" href="../filter.html">Filters</a></li>
<li class="toctree-l1"><a class="reference internal" href="../model.html">Models</a></li>
<li class="toctree-l1"><a class="reference internal" href="../geometry.html">Lie Groups</a></li>
<li class="toctree-l1"><a class="reference internal" href="../matlab.html">Matlab</a></li>
<li class="toctree-l1"><a class="reference internal" href="../license.html">License</a></li>
<li class="toctree-l1"><a class="reference internal" href="../bibliography.html">Bibliography</a></li>
</ul>

            
          
        </div>
      </div>
    </nav>

    <section data-toggle="wy-nav-shift" class="wy-nav-content-wrap">

      
      <nav class="wy-nav-top" role="navigation" aria-label="top navigation">
        <i data-toggle="wy-nav-top" class="fa fa-bars"></i>
        <a href="../index.html">Unscented Kalman Filtering on (Parallelizable) Manifolds</a>
      </nav>


      
      <div class="wy-nav-content">
        <div class="rst-content">
          

 



<div role="navigation" aria-label="breadcrumbs navigation">
  <ul class="wy-breadcrumbs">
    <li><a href="../index.html">Docs</a> &raquo;</li>
      
          <li><a href="../benchmarks.html">Benchmarks</a> &raquo;</li>
      
    <li>2D Robot SLAM - Benchmark</li>
      <li class="wy-breadcrumbs-aside">
        
          
            <a href="../_sources/auto_benchmark/slam2d.rst.txt" rel="nofollow"> View page source</a>
          
        
      </li>
  </ul>
  <hr/>
</div>
          <div role="main" class="document" itemscope="itemscope" itemtype="http://schema.org/Article">
           <div itemprop="articleBody">
            
  <div class="sphx-glr-download-link-note admonition note">
<p class="admonition-title">Note</p>
<p>Click <a class="reference internal" href="#sphx-glr-download-auto-benchmark-slam2d-py"><span class="std std-ref">here</span></a> to download the full example code</p>
</div>
<div class="sphx-glr-example-title section" id="d-robot-slam-benchmark">
<span id="sphx-glr-auto-benchmark-slam2d-py"></span><h1>2D Robot SLAM - Benchmark<a class="headerlink" href="#d-robot-slam-benchmark" title="Permalink to this headline">¶</a></h1>
<p>Goals of this script:</p>
<ul>
<li><p>implement three different UKFs on the 2D robot SLAM problem.</p></li>
<li><p>(re)discover computational alternatives for performing UKF:</p>
<blockquote>
<div><ul class="simple">
<li><p>when Jacobian are (partially) known.</p></li>
<li><p>or when only a part of the state is involved in a propagation step.</p></li>
<li><p>or when only a part of the state is involved in a measurement.</p></li>
</ul>
</div></blockquote>
</li>
<li><p>design the Extended Kalman Filter (EKF) and the Invariant Extended Kalman
Filter (IEKF) <a class="reference internal" href="../bibliography.html#barrauinvariant2017" id="id1">[BB17]</a>.</p></li>
<li><p>compare the different algorithms with Monte-Carlo simulations.</p></li>
</ul>
<p><em>We assume the reader is already familiar with the considered problem described
in the related example.</em></p>
<p>For the given, three different UKFs emerge, defined respectively as:</p>
<ol class="arabic">
<li><p>The state is embedded in <span class="math notranslate nohighlight">\(SO(2) \times \mathbb{R}^{2(1+L)}\)</span>, where:</p>
<ul class="simple">
<li><p>the retraction <span class="math notranslate nohighlight">\(\varphi(.,.)\)</span> is the <span class="math notranslate nohighlight">\(SO(2)\)</span> exponential map
for orientation and the vector addition for robot and landmark positions.</p></li>
<li><p>the inverse retraction <span class="math notranslate nohighlight">\(\varphi^{-1}(.,.)\)</span> is the <span class="math notranslate nohighlight">\(SO(2)\)</span>
logarithm for orientation and the vector subtraction for robot and landmark
positions.</p></li>
</ul>
</li>
<li><p>The state is embedded in <span class="math notranslate nohighlight">\(SE_{1+L}(2)\)</span> with left multiplication, i.e.</p>
<ul class="simple">
<li><p>the retraction <span class="math notranslate nohighlight">\(\varphi(.,.)\)</span> is the <span class="math notranslate nohighlight">\(SE_{1+L}(2)\)</span>
exponential, where the state multiplies on the left the uncertainty
<span class="math notranslate nohighlight">\(\boldsymbol{\xi}\)</span>.</p></li>
<li><p>the inverse retraction <span class="math notranslate nohighlight">\(\varphi^{-1}(.,.)\)</span> is the <span class="math notranslate nohighlight">\(SE_{1+L}
(2)\)</span> logarithm.</p></li>
</ul>
</li>
<li><p>The state is embedded in <span class="math notranslate nohighlight">\(SE_{1+L}(2)\)</span> with right multiplication, i.e.</p>
<blockquote>
<div><ul class="simple">
<li><p>the retraction <span class="math notranslate nohighlight">\(\varphi(.,.)\)</span> is the <span class="math notranslate nohighlight">\(SE_{1+L}(2)\)</span>
exponential, where state multiplies on the right the uncertainty
<span class="math notranslate nohighlight">\(\boldsymbol{\xi}\)</span>.</p></li>
<li><p>the inverse retraction <span class="math notranslate nohighlight">\(\varphi^{-1}(.,.)\)</span> is the <span class="math notranslate nohighlight">\(SE_{1+L}
(2)\)</span> logarithm.</p></li>
<li><p>it corresponds to the Invariant Extended Kalman Filter (IEKF) recommended
in  <a class="reference internal" href="../bibliography.html#barrauinvariant2017" id="id2">[BB17]</a> that naturally leads to resolve the
consistency issue of traditional EKF-SLAM.</p></li>
</ul>
</div></blockquote>
</li>
</ol>
<div class="admonition note">
<p class="admonition-title">Note</p>
<p><span class="math notranslate nohighlight">\(SE_{1+L}(2)\)</span> exponential and logarithm are similar as their
<span class="math notranslate nohighlight">\(SE(2)\)</span> counterpart, see <a class="reference internal" href="../geometry.html#geometry"><span class="std std-ref">documentation</span></a>.</p>
</div>
<div class="section" id="import">
<h2>Import<a class="headerlink" href="#import" title="Permalink to this headline">¶</a></h2>
<p>We import a specific EKF for performing state augmentation and update with
different measurement dimension. Indeed, in 2D SLAM, unknown landmarks are
progressively added to the state the first time the landmark is observing. And
each update consists of observed only visible landmarks. Both operations are
also handled in our advanced <code class="docutils literal notranslate"><span class="pre">JUKF</span></code>.</p>
<div class="highlight-default notranslate"><div class="highlight"><pre><span></span><span class="kn">from</span> <span class="nn">ukfm.model.slam2d</span> <span class="k">import</span> <span class="n">EKF</span>
<span class="kn">from</span> <span class="nn">ukfm</span> <span class="k">import</span> <span class="n">SO2</span><span class="p">,</span> <span class="n">JUKF</span>
<span class="kn">from</span> <span class="nn">ukfm</span> <span class="k">import</span> <span class="n">SLAM2D</span> <span class="k">as</span> <span class="n">MODEL</span>
<span class="kn">import</span> <span class="nn">numpy</span> <span class="k">as</span> <span class="nn">np</span>
<span class="kn">import</span> <span class="nn">ukfm</span>
<span class="n">ukfm</span><span class="o">.</span><span class="n">set_matplotlib_config</span><span class="p">()</span>
</pre></div>
</div>
</div>
<div class="section" id="simulation-setting">
<h2>Simulation Setting<a class="headerlink" href="#simulation-setting" title="Permalink to this headline">¶</a></h2>
<p>We compare the filters on a large number of Monte-Carlo runs.</p>
<div class="highlight-default notranslate"><div class="highlight"><pre><span></span><span class="c1"># Monte-Carlo runs</span>
<span class="n">N_mc</span> <span class="o">=</span> <span class="mi">100</span>
</pre></div>
</div>
<p>This script uses the <code class="docutils literal notranslate"><span class="pre">SLAM2D</span></code> model that requires sequence time and
odometry frequency.</p>
<div class="highlight-default notranslate"><div class="highlight"><pre><span></span><span class="c1"># sequence time (s)</span>
<span class="n">T</span> <span class="o">=</span> <span class="mi">2500</span>
<span class="c1"># odometry frequency (Hz)</span>
<span class="n">odo_freq</span> <span class="o">=</span> <span class="mi">1</span>
<span class="c1"># create the model</span>
<span class="n">model</span> <span class="o">=</span> <span class="n">MODEL</span><span class="p">(</span><span class="n">T</span><span class="p">,</span> <span class="n">odo_freq</span><span class="p">)</span>
</pre></div>
</div>
<p>The trajectory of the robot consists of turning at constant speed. The map
will be the same for all the simulation, where landmarks are constantly spaced
along the robot trajectory.</p>
<div class="highlight-default notranslate"><div class="highlight"><pre><span></span><span class="c1"># true speed of robot (m/s)</span>
<span class="n">v</span> <span class="o">=</span> <span class="mf">0.25</span>
<span class="c1"># true angular velocity (rad/s)</span>
<span class="n">gyro</span> <span class="o">=</span> <span class="mf">1.5</span><span class="o">/</span><span class="mi">180</span><span class="o">*</span><span class="n">np</span><span class="o">.</span><span class="n">pi</span>
<span class="c1"># odometry noise standard deviation</span>
<span class="n">odo_std</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">array</span><span class="p">([</span><span class="mf">0.05</span><span class="o">*</span><span class="n">v</span><span class="o">/</span><span class="n">np</span><span class="o">.</span><span class="n">sqrt</span><span class="p">(</span><span class="mi">2</span><span class="p">),</span>     <span class="c1"># speed (v/m)</span>
                    <span class="mf">0.05</span><span class="o">*</span><span class="n">v</span><span class="o">*</span><span class="n">np</span><span class="o">.</span><span class="n">sqrt</span><span class="p">(</span><span class="mi">2</span><span class="p">)</span><span class="o">*</span><span class="mi">2</span><span class="p">])</span>  <span class="c1"># angular speed (rad/s)</span>
<span class="c1"># observation noise standard deviation (m)</span>
<span class="n">obs_std</span> <span class="o">=</span> <span class="mf">0.1</span>
</pre></div>
</div>
</div>
<div class="section" id="filter-design">
<h2>Filter Design<a class="headerlink" href="#filter-design" title="Permalink to this headline">¶</a></h2>
<p>Additionally to the three UKFs, we compare them to an EKF and an IEKF. The EKF
has the same uncertainty representation as the UKF with <span class="math notranslate nohighlight">\(SO(2) \times
\mathbb{R}^{2(1+L)}\)</span> uncertainty representation, whereas the IEKF has the
same uncertainty representation as the UKF with right <span class="math notranslate nohighlight">\(SE_{1+L}(2)\)</span>
retraction.</p>
<p>We have five similar methods, but the UKF implementations slightly differs.
Indeed, using our vanilla UKF works for all choice of retraction but is not
adapted to the problem from a computationally point of view. And we spare
computation only when Jacobian is known.</p>
<div class="highlight-default notranslate"><div class="highlight"><pre><span></span><span class="c1"># propagation noise covariance matrix</span>
<span class="n">Q</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">diag</span><span class="p">(</span><span class="n">odo_std</span><span class="o">**</span><span class="mi">2</span><span class="p">)</span>
<span class="c1"># measurement noise covariance matrix</span>
<span class="n">R</span> <span class="o">=</span> <span class="n">obs_std</span><span class="o">**</span><span class="mi">2</span><span class="o">*</span><span class="n">np</span><span class="o">.</span><span class="n">eye</span><span class="p">(</span><span class="mi">2</span><span class="p">)</span>
<span class="c1"># initial uncertainty matrix</span>
<span class="n">P0</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">zeros</span><span class="p">((</span><span class="mi">3</span><span class="p">,</span> <span class="mi">3</span><span class="p">))</span>
<span class="c1"># sigma point parameter</span>
<span class="n">alpha</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">array</span><span class="p">([</span><span class="mf">1e-3</span><span class="p">,</span> <span class="mf">1e-3</span><span class="p">,</span> <span class="mf">1e-3</span><span class="p">,</span> <span class="mf">1e-3</span><span class="p">,</span> <span class="mf">1e-3</span><span class="p">])</span>
<span class="n">red_idxs</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">array</span><span class="p">([</span><span class="mi">0</span><span class="p">,</span> <span class="mi">1</span><span class="p">,</span> <span class="mi">2</span><span class="p">])</span>  <span class="c1"># indices corresponding to the robot state in P</span>
<span class="n">aug_idxs</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">array</span><span class="p">([</span><span class="mi">0</span><span class="p">,</span> <span class="mi">1</span><span class="p">,</span> <span class="mi">2</span><span class="p">])</span>  <span class="c1"># indices corresponding to the robot state in P</span>
</pre></div>
</div>
<p>We set variables for recording metrics before launching Monte-Carlo
simulations.</p>
<div class="highlight-default notranslate"><div class="highlight"><pre><span></span><span class="n">ukf_err</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">zeros</span><span class="p">((</span><span class="n">N_mc</span><span class="p">,</span> <span class="n">model</span><span class="o">.</span><span class="n">N</span><span class="p">,</span> <span class="mi">3</span><span class="p">))</span>
<span class="n">left_ukf_err</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">zeros_like</span><span class="p">(</span><span class="n">ukf_err</span><span class="p">)</span>
<span class="n">right_ukf_err</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">zeros_like</span><span class="p">(</span><span class="n">ukf_err</span><span class="p">)</span>
<span class="n">iekf_err</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">zeros_like</span><span class="p">(</span><span class="n">ukf_err</span><span class="p">)</span>
<span class="n">ekf_err</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">zeros_like</span><span class="p">(</span><span class="n">ukf_err</span><span class="p">)</span>

<span class="n">ukf_nees</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">zeros</span><span class="p">((</span><span class="n">N_mc</span><span class="p">,</span> <span class="n">model</span><span class="o">.</span><span class="n">N</span><span class="p">,</span> <span class="mi">2</span><span class="p">))</span>
<span class="n">left_ukf_nees</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">zeros_like</span><span class="p">(</span><span class="n">ukf_nees</span><span class="p">)</span>
<span class="n">right_ukf_nees</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">zeros_like</span><span class="p">(</span><span class="n">ukf_nees</span><span class="p">)</span>
<span class="n">iekf_nees</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">zeros_like</span><span class="p">(</span><span class="n">ukf_nees</span><span class="p">)</span>
<span class="n">ekf_nees</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">zeros_like</span><span class="p">(</span><span class="n">ukf_nees</span><span class="p">)</span>
</pre></div>
</div>
</div>
<div class="section" id="monte-carlo-runs">
<h2>Monte-Carlo Runs<a class="headerlink" href="#monte-carlo-runs" title="Permalink to this headline">¶</a></h2>
<p>We run the Monte-Carlo through a for loop.</p>
<div class="highlight-default notranslate"><div class="highlight"><pre><span></span><span class="k">for</span> <span class="n">n_mc</span> <span class="ow">in</span> <span class="nb">range</span><span class="p">(</span><span class="n">N_mc</span><span class="p">):</span>
    <span class="nb">print</span><span class="p">(</span><span class="s2">&quot;Monte-Carlo iteration(s): &quot;</span> <span class="o">+</span> <span class="nb">str</span><span class="p">(</span><span class="n">n_mc</span><span class="o">+</span><span class="mi">1</span><span class="p">)</span> <span class="o">+</span> <span class="s2">&quot;/&quot;</span> <span class="o">+</span> <span class="nb">str</span><span class="p">(</span><span class="n">N_mc</span><span class="p">))</span>
    <span class="c1"># simulate true trajectory and noisy input</span>
    <span class="n">states</span><span class="p">,</span> <span class="n">omegas</span><span class="p">,</span> <span class="n">ldks</span> <span class="o">=</span> <span class="n">model</span><span class="o">.</span><span class="n">simu_f</span><span class="p">(</span><span class="n">odo_std</span><span class="p">,</span> <span class="n">v</span><span class="p">,</span> <span class="n">gyro</span><span class="p">)</span>
    <span class="c1"># simulate landmark measurements</span>
    <span class="n">ys</span> <span class="o">=</span> <span class="n">model</span><span class="o">.</span><span class="n">simu_h</span><span class="p">(</span><span class="n">states</span><span class="p">,</span> <span class="n">obs_std</span><span class="p">,</span> <span class="n">ldks</span><span class="p">)</span>
    <span class="c1"># initialize filter with true state</span>
    <span class="n">state0</span> <span class="o">=</span> <span class="n">model</span><span class="o">.</span><span class="n">STATE</span><span class="p">(</span>
        <span class="n">Rot</span><span class="o">=</span><span class="n">states</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span><span class="o">.</span><span class="n">Rot</span><span class="p">,</span>
        <span class="n">p</span><span class="o">=</span><span class="n">states</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span><span class="o">.</span><span class="n">p</span><span class="p">,</span>
        <span class="n">p_l</span><span class="o">=</span><span class="n">np</span><span class="o">.</span><span class="n">zeros</span><span class="p">((</span><span class="mi">0</span><span class="p">,</span> <span class="mi">2</span><span class="p">))</span>
    <span class="p">)</span>

    <span class="n">ukf</span> <span class="o">=</span> <span class="n">JUKF</span><span class="p">(</span><span class="n">state0</span><span class="o">=</span><span class="n">state0</span><span class="p">,</span> <span class="n">P0</span><span class="o">=</span><span class="n">P0</span><span class="p">,</span> <span class="n">f</span><span class="o">=</span><span class="n">model</span><span class="o">.</span><span class="n">f</span><span class="p">,</span> <span class="n">h</span><span class="o">=</span><span class="n">model</span><span class="o">.</span><span class="n">h</span><span class="p">,</span> <span class="n">Q</span><span class="o">=</span><span class="n">Q</span><span class="p">,</span> <span class="n">phi</span><span class="o">=</span><span class="n">model</span><span class="o">.</span><span class="n">phi</span><span class="p">,</span>
               <span class="n">alpha</span><span class="o">=</span><span class="n">alpha</span><span class="p">,</span> <span class="n">red_phi</span><span class="o">=</span><span class="n">model</span><span class="o">.</span><span class="n">red_phi</span><span class="p">,</span>
               <span class="n">red_phi_inv</span><span class="o">=</span><span class="n">model</span><span class="o">.</span><span class="n">red_phi_inv</span><span class="p">,</span> <span class="n">red_idxs</span><span class="o">=</span><span class="n">red_idxs</span><span class="p">,</span>
               <span class="n">up_phi</span><span class="o">=</span><span class="n">model</span><span class="o">.</span><span class="n">up_phi</span><span class="p">,</span> <span class="n">up_idxs</span><span class="o">=</span><span class="n">np</span><span class="o">.</span><span class="n">arange</span><span class="p">(</span><span class="mi">5</span><span class="p">),</span> <span class="n">aug_z</span><span class="o">=</span><span class="n">model</span><span class="o">.</span><span class="n">aug_z</span><span class="p">,</span>
               <span class="n">aug_phi</span><span class="o">=</span><span class="n">model</span><span class="o">.</span><span class="n">aug_phi</span><span class="p">,</span> <span class="n">aug_phi_inv</span><span class="o">=</span><span class="n">model</span><span class="o">.</span><span class="n">aug_phi_inv</span><span class="p">,</span>
               <span class="n">aug_idxs</span><span class="o">=</span><span class="n">aug_idxs</span><span class="p">,</span> <span class="n">aug_q</span><span class="o">=</span><span class="mi">2</span><span class="p">)</span>
    <span class="n">left_ukf</span> <span class="o">=</span> <span class="n">JUKF</span><span class="p">(</span><span class="n">state0</span><span class="o">=</span><span class="n">state0</span><span class="p">,</span> <span class="n">P0</span><span class="o">=</span><span class="n">P0</span><span class="p">,</span> <span class="n">f</span><span class="o">=</span><span class="n">model</span><span class="o">.</span><span class="n">f</span><span class="p">,</span> <span class="n">h</span><span class="o">=</span><span class="n">model</span><span class="o">.</span><span class="n">h</span><span class="p">,</span> <span class="n">Q</span><span class="o">=</span><span class="n">Q</span><span class="p">,</span>
                    <span class="n">phi</span><span class="o">=</span><span class="n">model</span><span class="o">.</span><span class="n">left_phi</span><span class="p">,</span> <span class="n">alpha</span><span class="o">=</span><span class="n">alpha</span><span class="p">,</span> <span class="n">red_phi</span><span class="o">=</span><span class="n">model</span><span class="o">.</span><span class="n">left_red_phi</span><span class="p">,</span>
                    <span class="n">red_phi_inv</span><span class="o">=</span><span class="n">model</span><span class="o">.</span><span class="n">left_red_phi_inv</span><span class="p">,</span> <span class="n">red_idxs</span><span class="o">=</span><span class="n">red_idxs</span><span class="p">,</span>
                    <span class="n">up_phi</span><span class="o">=</span><span class="n">model</span><span class="o">.</span><span class="n">left_up_phi</span><span class="p">,</span> <span class="n">up_idxs</span><span class="o">=</span><span class="n">np</span><span class="o">.</span><span class="n">arange</span><span class="p">(</span><span class="mi">5</span><span class="p">),</span>
                    <span class="n">aug_z</span><span class="o">=</span><span class="n">model</span><span class="o">.</span><span class="n">aug_z</span><span class="p">,</span> <span class="n">aug_phi</span><span class="o">=</span><span class="n">model</span><span class="o">.</span><span class="n">left_aug_phi</span><span class="p">,</span>
                    <span class="n">aug_phi_inv</span><span class="o">=</span><span class="n">model</span><span class="o">.</span><span class="n">left_aug_phi_inv</span><span class="p">,</span> <span class="n">aug_idxs</span><span class="o">=</span><span class="n">aug_idxs</span><span class="p">,</span>
                    <span class="n">aug_q</span><span class="o">=</span><span class="mi">2</span><span class="p">)</span>
    <span class="n">right_ukf</span> <span class="o">=</span> <span class="n">JUKF</span><span class="p">(</span><span class="n">state0</span><span class="o">=</span><span class="n">state0</span><span class="p">,</span> <span class="n">P0</span><span class="o">=</span><span class="n">P0</span><span class="p">,</span> <span class="n">f</span><span class="o">=</span><span class="n">model</span><span class="o">.</span><span class="n">f</span><span class="p">,</span> <span class="n">h</span><span class="o">=</span><span class="n">model</span><span class="o">.</span><span class="n">h</span><span class="p">,</span> <span class="n">Q</span><span class="o">=</span><span class="n">Q</span><span class="p">,</span>
                     <span class="n">phi</span><span class="o">=</span><span class="n">model</span><span class="o">.</span><span class="n">right_phi</span><span class="p">,</span> <span class="n">alpha</span><span class="o">=</span><span class="n">alpha</span><span class="p">,</span> <span class="n">aug_z</span><span class="o">=</span><span class="n">model</span><span class="o">.</span><span class="n">aug_z</span><span class="p">,</span>
                     <span class="n">red_phi</span><span class="o">=</span><span class="n">model</span><span class="o">.</span><span class="n">right_red_phi</span><span class="p">,</span>
                     <span class="n">red_phi_inv</span><span class="o">=</span><span class="n">model</span><span class="o">.</span><span class="n">right_red_phi_inv</span><span class="p">,</span> <span class="n">red_idxs</span><span class="o">=</span><span class="n">red_idxs</span><span class="p">,</span>
                     <span class="n">up_phi</span><span class="o">=</span><span class="n">model</span><span class="o">.</span><span class="n">right_up_phi</span><span class="p">,</span> <span class="n">up_idxs</span><span class="o">=</span><span class="n">np</span><span class="o">.</span><span class="n">arange</span><span class="p">(</span><span class="mi">5</span><span class="p">),</span>
                     <span class="n">aug_phi</span><span class="o">=</span><span class="n">model</span><span class="o">.</span><span class="n">right_aug_phi</span><span class="p">,</span>
                     <span class="n">aug_phi_inv</span><span class="o">=</span><span class="n">model</span><span class="o">.</span><span class="n">right_aug_phi_inv</span><span class="p">,</span>
                     <span class="n">aug_idxs</span><span class="o">=</span><span class="n">aug_idxs</span><span class="p">,</span> <span class="n">aug_q</span><span class="o">=</span><span class="mi">2</span><span class="p">)</span>
    <span class="n">iekf</span> <span class="o">=</span> <span class="n">EKF</span><span class="p">(</span><span class="n">state0</span><span class="o">=</span><span class="n">state0</span><span class="p">,</span> <span class="n">P0</span><span class="o">=</span><span class="n">P0</span><span class="p">,</span> <span class="n">f</span><span class="o">=</span><span class="n">model</span><span class="o">.</span><span class="n">f</span><span class="p">,</span> <span class="n">h</span><span class="o">=</span><span class="n">model</span><span class="o">.</span><span class="n">h</span><span class="p">,</span> <span class="n">Q</span><span class="o">=</span><span class="n">Q</span><span class="p">,</span>
               <span class="n">phi</span><span class="o">=</span><span class="n">model</span><span class="o">.</span><span class="n">right_phi</span><span class="p">,</span> <span class="n">z</span><span class="o">=</span><span class="n">model</span><span class="o">.</span><span class="n">z</span><span class="p">,</span> <span class="n">aug_z</span><span class="o">=</span><span class="n">model</span><span class="o">.</span><span class="n">aug_z</span><span class="p">)</span>
    <span class="n">iekf</span><span class="o">.</span><span class="n">jacobian_propagation</span> <span class="o">=</span> <span class="n">iekf</span><span class="o">.</span><span class="n">iekf_FG_ana</span>
    <span class="n">iekf</span><span class="o">.</span><span class="n">H_num</span> <span class="o">=</span> <span class="n">iekf</span><span class="o">.</span><span class="n">iekf_jacobian_update</span>
    <span class="n">iekf</span><span class="o">.</span><span class="n">aug</span> <span class="o">=</span> <span class="n">iekf</span><span class="o">.</span><span class="n">iekf_augment</span>

    <span class="n">ekf</span> <span class="o">=</span> <span class="n">EKF</span><span class="p">(</span><span class="n">state0</span><span class="o">=</span><span class="n">state0</span><span class="p">,</span> <span class="n">P0</span><span class="o">=</span><span class="n">P0</span><span class="p">,</span> <span class="n">f</span><span class="o">=</span><span class="n">model</span><span class="o">.</span><span class="n">f</span><span class="p">,</span> <span class="n">h</span><span class="o">=</span><span class="n">model</span><span class="o">.</span><span class="n">h</span><span class="p">,</span> <span class="n">Q</span><span class="o">=</span><span class="n">Q</span><span class="p">,</span>
              <span class="n">phi</span><span class="o">=</span><span class="n">model</span><span class="o">.</span><span class="n">phi</span><span class="p">,</span> <span class="n">z</span><span class="o">=</span><span class="n">model</span><span class="o">.</span><span class="n">z</span><span class="p">,</span> <span class="n">aug_z</span><span class="o">=</span><span class="n">model</span><span class="o">.</span><span class="n">aug_z</span><span class="p">)</span>
    <span class="n">ekf</span><span class="o">.</span><span class="n">jacobian_propagation</span> <span class="o">=</span> <span class="n">ekf</span><span class="o">.</span><span class="n">ekf_FG_ana</span>
    <span class="n">ekf</span><span class="o">.</span><span class="n">H_num</span> <span class="o">=</span> <span class="n">ekf</span><span class="o">.</span><span class="n">ekf_jacobian_update</span>
    <span class="n">ekf</span><span class="o">.</span><span class="n">aug</span> <span class="o">=</span> <span class="n">ekf</span><span class="o">.</span><span class="n">ekf_augment</span>

    <span class="n">ukf_states</span> <span class="o">=</span> <span class="p">[</span><span class="n">state0</span><span class="p">]</span>
    <span class="n">left_ukf_states</span> <span class="o">=</span> <span class="p">[</span><span class="n">state0</span><span class="p">]</span>
    <span class="n">right_ukf_states</span> <span class="o">=</span> <span class="p">[</span><span class="n">state0</span><span class="p">]</span>
    <span class="n">iekf_states</span> <span class="o">=</span> <span class="p">[</span><span class="n">state0</span><span class="p">]</span>
    <span class="n">ekf_states</span> <span class="o">=</span> <span class="p">[</span><span class="n">state0</span><span class="p">]</span>

    <span class="n">ukf_Ps</span> <span class="o">=</span> <span class="p">[</span><span class="n">P0</span><span class="p">]</span>
    <span class="n">left_ukf_Ps</span> <span class="o">=</span> <span class="p">[</span><span class="n">P0</span><span class="p">]</span>
    <span class="n">right_ukf_Ps</span> <span class="o">=</span> <span class="p">[</span><span class="n">P0</span><span class="p">]</span>
    <span class="n">ekf_Ps</span> <span class="o">=</span> <span class="p">[</span><span class="n">P0</span><span class="p">]</span>
    <span class="n">iekf_Ps</span> <span class="o">=</span> <span class="p">[</span><span class="n">P0</span><span class="p">]</span>

    <span class="c1"># indices of already observed landmarks</span>
    <span class="n">ukf_lmk</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">array</span><span class="p">([])</span>

    <span class="c1"># The UKF proceeds as a standard Kalman filter with a for loop.</span>
    <span class="k">for</span> <span class="n">n</span> <span class="ow">in</span> <span class="nb">range</span><span class="p">(</span><span class="mi">1</span><span class="p">,</span> <span class="n">model</span><span class="o">.</span><span class="n">N</span><span class="p">):</span>
        <span class="c1"># propagation</span>
        <span class="n">ukf</span><span class="o">.</span><span class="n">propagation</span><span class="p">(</span><span class="n">omegas</span><span class="p">[</span><span class="n">n</span><span class="o">-</span><span class="mi">1</span><span class="p">],</span> <span class="n">model</span><span class="o">.</span><span class="n">dt</span><span class="p">)</span>
        <span class="n">left_ukf</span><span class="o">.</span><span class="n">red_d</span> <span class="o">=</span> <span class="n">left_ukf</span><span class="o">.</span><span class="n">P</span><span class="o">.</span><span class="n">shape</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span>
        <span class="n">left_ukf</span><span class="o">.</span><span class="n">red_idxs</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">arange</span><span class="p">(</span><span class="n">left_ukf</span><span class="o">.</span><span class="n">P</span><span class="o">.</span><span class="n">shape</span><span class="p">[</span><span class="mi">0</span><span class="p">])</span>
        <span class="n">left_ukf</span><span class="o">.</span><span class="n">red_d</span> <span class="o">=</span> <span class="n">left_ukf</span><span class="o">.</span><span class="n">red_idxs</span><span class="o">.</span><span class="n">shape</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span>
        <span class="n">left_ukf</span><span class="o">.</span><span class="n">weights</span> <span class="o">=</span> <span class="n">left_ukf</span><span class="o">.</span><span class="n">WEIGHTS</span><span class="p">(</span><span class="n">left_ukf</span><span class="o">.</span><span class="n">red_d</span><span class="p">,</span>
                                            <span class="n">left_ukf</span><span class="o">.</span><span class="n">Q</span><span class="o">.</span><span class="n">shape</span><span class="p">[</span><span class="mi">0</span><span class="p">],</span> <span class="n">left_ukf</span><span class="o">.</span><span class="n">up_d</span><span class="p">,</span>
                                            <span class="n">left_ukf</span><span class="o">.</span><span class="n">aug_d</span><span class="p">,</span> <span class="n">left_ukf</span><span class="o">.</span><span class="n">aug_q</span><span class="p">,</span> <span class="n">alpha</span><span class="p">)</span>
        <span class="n">left_ukf</span><span class="o">.</span><span class="n">propagation</span><span class="p">(</span><span class="n">omegas</span><span class="p">[</span><span class="n">n</span><span class="o">-</span><span class="mi">1</span><span class="p">],</span> <span class="n">model</span><span class="o">.</span><span class="n">dt</span><span class="p">)</span>
        <span class="n">iekf</span><span class="o">.</span><span class="n">propagation</span><span class="p">(</span><span class="n">omegas</span><span class="p">[</span><span class="n">n</span><span class="o">-</span><span class="mi">1</span><span class="p">],</span> <span class="n">model</span><span class="o">.</span><span class="n">dt</span><span class="p">)</span>
        <span class="n">ekf</span><span class="o">.</span><span class="n">propagation</span><span class="p">(</span><span class="n">omegas</span><span class="p">[</span><span class="n">n</span><span class="o">-</span><span class="mi">1</span><span class="p">],</span> <span class="n">model</span><span class="o">.</span><span class="n">dt</span><span class="p">)</span>
        <span class="c1"># propagation of right Jacobian</span>
        <span class="n">right_ukf</span><span class="o">.</span><span class="n">state_propagation</span><span class="p">(</span><span class="n">omegas</span><span class="p">[</span><span class="n">n</span><span class="o">-</span><span class="mi">1</span><span class="p">],</span> <span class="n">model</span><span class="o">.</span><span class="n">dt</span><span class="p">)</span>
        <span class="n">right_ukf</span><span class="o">.</span><span class="n">F</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">eye</span><span class="p">(</span><span class="n">right_ukf</span><span class="o">.</span><span class="n">P</span><span class="o">.</span><span class="n">shape</span><span class="p">[</span><span class="mi">0</span><span class="p">])</span>
        <span class="n">right_ukf</span><span class="o">.</span><span class="n">red_d</span> <span class="o">=</span> <span class="n">right_ukf</span><span class="o">.</span><span class="n">P</span><span class="o">.</span><span class="n">shape</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span>
        <span class="n">right_ukf</span><span class="o">.</span><span class="n">red_idxs</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">arange</span><span class="p">(</span><span class="n">right_ukf</span><span class="o">.</span><span class="n">P</span><span class="o">.</span><span class="n">shape</span><span class="p">[</span><span class="mi">0</span><span class="p">])</span>
        <span class="n">right_ukf</span><span class="o">.</span><span class="n">G_num</span><span class="p">(</span><span class="n">omegas</span><span class="p">[</span><span class="n">n</span><span class="o">-</span><span class="mi">1</span><span class="p">],</span> <span class="n">model</span><span class="o">.</span><span class="n">dt</span><span class="p">)</span>
        <span class="n">right_ukf</span><span class="o">.</span><span class="n">cov_propagation</span><span class="p">()</span>
        <span class="n">y_n</span> <span class="o">=</span> <span class="n">ys</span><span class="p">[</span><span class="n">n</span><span class="p">]</span>
        <span class="c1"># observed landmarks</span>
        <span class="n">idxs</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">where</span><span class="p">(</span><span class="n">y_n</span><span class="p">[:,</span> <span class="mi">2</span><span class="p">]</span> <span class="o">&gt;=</span> <span class="mi">0</span><span class="p">)[</span><span class="mi">0</span><span class="p">]</span>
        <span class="c1"># update each landmark already in the filter</span>
        <span class="n">p_ls</span> <span class="o">=</span> <span class="n">ukf</span><span class="o">.</span><span class="n">state</span><span class="o">.</span><span class="n">p_l</span>
        <span class="n">left_p_ls</span> <span class="o">=</span> <span class="n">left_ukf</span><span class="o">.</span><span class="n">state</span><span class="o">.</span><span class="n">p_l</span>
        <span class="n">right_p_ls</span> <span class="o">=</span> <span class="n">right_ukf</span><span class="o">.</span><span class="n">state</span><span class="o">.</span><span class="n">p_l</span>
        <span class="n">iekf_p_ls</span> <span class="o">=</span> <span class="n">iekf</span><span class="o">.</span><span class="n">state</span><span class="o">.</span><span class="n">p_l</span>
        <span class="n">ekf_p_ls</span> <span class="o">=</span> <span class="n">ekf</span><span class="o">.</span><span class="n">state</span><span class="o">.</span><span class="n">p_l</span>
        <span class="k">for</span> <span class="n">idx0</span> <span class="ow">in</span> <span class="n">idxs</span><span class="p">:</span>
            <span class="n">idx</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">where</span><span class="p">(</span><span class="n">ukf_lmk</span> <span class="o">==</span> <span class="n">y_n</span><span class="p">[</span><span class="n">idx0</span><span class="p">,</span> <span class="mi">2</span><span class="p">])[</span><span class="mi">0</span><span class="p">]</span>
            <span class="k">if</span> <span class="n">idx</span><span class="o">.</span><span class="n">shape</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span> <span class="o">==</span> <span class="mi">0</span><span class="p">:</span>
                <span class="k">continue</span>
            <span class="c1"># indices of the robot and observed landmark in P</span>
            <span class="n">up_idxs</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">hstack</span><span class="p">([</span><span class="mi">0</span><span class="p">,</span> <span class="mi">1</span><span class="p">,</span> <span class="mi">2</span><span class="p">,</span> <span class="mi">3</span><span class="o">+</span><span class="mi">2</span><span class="o">*</span><span class="n">idx</span><span class="p">,</span> <span class="mi">4</span><span class="o">+</span><span class="mi">2</span><span class="o">*</span><span class="n">idx</span><span class="p">])</span>
            <span class="n">ukf</span><span class="o">.</span><span class="n">state</span><span class="o">.</span><span class="n">p_l</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">squeeze</span><span class="p">(</span><span class="n">p_ls</span><span class="p">[</span><span class="n">idx</span><span class="p">])</span>
            <span class="n">left_ukf</span><span class="o">.</span><span class="n">state</span><span class="o">.</span><span class="n">p_l</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">squeeze</span><span class="p">(</span><span class="n">left_p_ls</span><span class="p">[</span><span class="n">idx</span><span class="p">])</span>
            <span class="n">right_ukf</span><span class="o">.</span><span class="n">state</span><span class="o">.</span><span class="n">p_l</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">squeeze</span><span class="p">(</span><span class="n">right_p_ls</span><span class="p">[</span><span class="n">idx</span><span class="p">])</span>
            <span class="n">iekf</span><span class="o">.</span><span class="n">state</span><span class="o">.</span><span class="n">p_l</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">squeeze</span><span class="p">(</span><span class="n">iekf_p_ls</span><span class="p">[</span><span class="n">idx</span><span class="p">])</span>
            <span class="n">ekf</span><span class="o">.</span><span class="n">state</span><span class="o">.</span><span class="n">p_l</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">squeeze</span><span class="p">(</span><span class="n">ekf_p_ls</span><span class="p">[</span><span class="n">idx</span><span class="p">])</span>
            <span class="c1"># compute observability matrices and residual</span>
            <span class="n">ukf</span><span class="o">.</span><span class="n">H_num</span><span class="p">(</span><span class="n">np</span><span class="o">.</span><span class="n">squeeze</span><span class="p">(</span><span class="n">y_n</span><span class="p">[</span><span class="n">idx0</span><span class="p">,</span> <span class="p">:</span><span class="mi">2</span><span class="p">]),</span> <span class="n">up_idxs</span><span class="p">,</span> <span class="n">R</span><span class="p">)</span>
            <span class="n">left_ukf</span><span class="o">.</span><span class="n">H_num</span><span class="p">(</span><span class="n">np</span><span class="o">.</span><span class="n">squeeze</span><span class="p">(</span><span class="n">y_n</span><span class="p">[</span><span class="n">idx0</span><span class="p">,</span> <span class="p">:</span><span class="mi">2</span><span class="p">]),</span> <span class="n">up_idxs</span><span class="p">,</span> <span class="n">R</span><span class="p">)</span>
            <span class="n">right_ukf</span><span class="o">.</span><span class="n">H_num</span><span class="p">(</span><span class="n">np</span><span class="o">.</span><span class="n">squeeze</span><span class="p">(</span><span class="n">y_n</span><span class="p">[</span><span class="n">idx0</span><span class="p">,</span> <span class="p">:</span><span class="mi">2</span><span class="p">]),</span> <span class="n">up_idxs</span><span class="p">,</span> <span class="n">R</span><span class="p">)</span>
            <span class="n">iekf</span><span class="o">.</span><span class="n">H_num</span><span class="p">(</span><span class="n">np</span><span class="o">.</span><span class="n">squeeze</span><span class="p">(</span><span class="n">y_n</span><span class="p">[</span><span class="n">idx0</span><span class="p">,</span> <span class="p">:</span><span class="mi">2</span><span class="p">]),</span> <span class="n">up_idxs</span><span class="p">,</span> <span class="n">R</span><span class="p">)</span>
            <span class="n">ekf</span><span class="o">.</span><span class="n">H_num</span><span class="p">(</span><span class="n">np</span><span class="o">.</span><span class="n">squeeze</span><span class="p">(</span><span class="n">y_n</span><span class="p">[</span><span class="n">idx0</span><span class="p">,</span> <span class="p">:</span><span class="mi">2</span><span class="p">]),</span> <span class="n">up_idxs</span><span class="p">,</span> <span class="n">R</span><span class="p">)</span>
        <span class="n">ukf</span><span class="o">.</span><span class="n">state</span><span class="o">.</span><span class="n">p_l</span> <span class="o">=</span> <span class="n">p_ls</span>
        <span class="n">left_ukf</span><span class="o">.</span><span class="n">state</span><span class="o">.</span><span class="n">p_l</span> <span class="o">=</span> <span class="n">left_p_ls</span>
        <span class="n">right_ukf</span><span class="o">.</span><span class="n">state</span><span class="o">.</span><span class="n">p_l</span> <span class="o">=</span> <span class="n">right_p_ls</span>
        <span class="n">iekf</span><span class="o">.</span><span class="n">state</span><span class="o">.</span><span class="n">p_l</span> <span class="o">=</span> <span class="n">iekf_p_ls</span>
        <span class="n">ekf</span><span class="o">.</span><span class="n">state</span><span class="o">.</span><span class="n">p_l</span> <span class="o">=</span> <span class="n">ekf_p_ls</span>
        <span class="c1"># update only if some landmarks have been observed</span>
        <span class="k">if</span> <span class="n">ukf</span><span class="o">.</span><span class="n">H</span><span class="o">.</span><span class="n">shape</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span> <span class="o">&gt;</span> <span class="mi">0</span><span class="p">:</span>
            <span class="n">ukf</span><span class="o">.</span><span class="n">state_update</span><span class="p">()</span>
            <span class="n">left_ukf</span><span class="o">.</span><span class="n">state_update</span><span class="p">()</span>
            <span class="n">right_ukf</span><span class="o">.</span><span class="n">state_update</span><span class="p">()</span>
            <span class="n">iekf</span><span class="o">.</span><span class="n">state_update</span><span class="p">()</span>
            <span class="n">ekf</span><span class="o">.</span><span class="n">state_update</span><span class="p">()</span>
        <span class="c1"># augment the state with new landmark</span>
        <span class="k">for</span> <span class="n">idx0</span> <span class="ow">in</span> <span class="n">idxs</span><span class="p">:</span>
            <span class="n">idx</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">where</span><span class="p">(</span><span class="n">ukf_lmk</span> <span class="o">==</span> <span class="n">y_n</span><span class="p">[</span><span class="n">idx0</span><span class="p">,</span> <span class="mi">2</span><span class="p">])[</span><span class="mi">0</span><span class="p">]</span>
            <span class="k">if</span> <span class="ow">not</span> <span class="n">idx</span><span class="o">.</span><span class="n">shape</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span> <span class="o">==</span> <span class="mi">0</span><span class="p">:</span>
                <span class="k">continue</span>
            <span class="c1"># augment the landmark state</span>
            <span class="n">ukf_lmk</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">hstack</span><span class="p">([</span><span class="n">ukf_lmk</span><span class="p">,</span> <span class="nb">int</span><span class="p">(</span><span class="n">y_n</span><span class="p">[</span><span class="n">idx0</span><span class="p">,</span> <span class="mi">2</span><span class="p">])])</span>
            <span class="c1"># indices of the new landmark</span>
            <span class="n">idx</span> <span class="o">=</span> <span class="n">ukf_lmk</span><span class="o">.</span><span class="n">shape</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span> <span class="o">-</span> <span class="mi">1</span>
            <span class="c1"># new landmark position</span>
            <span class="n">p_l</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">expand_dims</span><span class="p">(</span>
                <span class="n">ukf</span><span class="o">.</span><span class="n">state</span><span class="o">.</span><span class="n">p</span> <span class="o">+</span> <span class="n">ukf</span><span class="o">.</span><span class="n">state</span><span class="o">.</span><span class="n">Rot</span><span class="o">.</span><span class="n">dot</span><span class="p">(</span><span class="n">y_n</span><span class="p">[</span><span class="n">idx0</span><span class="p">,</span> <span class="p">:</span><span class="mi">2</span><span class="p">]),</span> <span class="mi">0</span><span class="p">)</span>
            <span class="n">left_p_l</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">expand_dims</span><span class="p">(</span>
                <span class="n">left_ukf</span><span class="o">.</span><span class="n">state</span><span class="o">.</span><span class="n">p</span> <span class="o">+</span> <span class="n">left_ukf</span><span class="o">.</span><span class="n">state</span><span class="o">.</span><span class="n">Rot</span><span class="o">.</span><span class="n">dot</span><span class="p">(</span><span class="n">y_n</span><span class="p">[</span><span class="n">idx0</span><span class="p">,</span> <span class="p">:</span><span class="mi">2</span><span class="p">]),</span> <span class="mi">0</span><span class="p">)</span>
            <span class="n">right_p_l</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">expand_dims</span><span class="p">(</span>
                <span class="n">right_ukf</span><span class="o">.</span><span class="n">state</span><span class="o">.</span><span class="n">p</span> <span class="o">+</span> <span class="n">right_ukf</span><span class="o">.</span><span class="n">state</span><span class="o">.</span><span class="n">Rot</span><span class="o">.</span><span class="n">dot</span><span class="p">(</span><span class="n">y_n</span><span class="p">[</span><span class="n">idx0</span><span class="p">,</span> <span class="p">:</span><span class="mi">2</span><span class="p">]),</span> <span class="mi">0</span><span class="p">)</span>
            <span class="n">iekf_p_l</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">expand_dims</span><span class="p">(</span>
                <span class="n">iekf</span><span class="o">.</span><span class="n">state</span><span class="o">.</span><span class="n">p</span> <span class="o">+</span> <span class="n">iekf</span><span class="o">.</span><span class="n">state</span><span class="o">.</span><span class="n">Rot</span><span class="o">.</span><span class="n">dot</span><span class="p">(</span><span class="n">y_n</span><span class="p">[</span><span class="n">idx0</span><span class="p">,</span> <span class="p">:</span><span class="mi">2</span><span class="p">]),</span> <span class="mi">0</span><span class="p">)</span>
            <span class="n">ekf_p_l</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">expand_dims</span><span class="p">(</span>
                <span class="n">ekf</span><span class="o">.</span><span class="n">state</span><span class="o">.</span><span class="n">p</span> <span class="o">+</span> <span class="n">ekf</span><span class="o">.</span><span class="n">state</span><span class="o">.</span><span class="n">Rot</span><span class="o">.</span><span class="n">dot</span><span class="p">(</span><span class="n">y_n</span><span class="p">[</span><span class="n">idx0</span><span class="p">,</span> <span class="p">:</span><span class="mi">2</span><span class="p">]),</span> <span class="mi">0</span><span class="p">)</span>
            <span class="n">p_ls</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">vstack</span><span class="p">([</span><span class="n">ukf</span><span class="o">.</span><span class="n">state</span><span class="o">.</span><span class="n">p_l</span><span class="p">,</span> <span class="n">p_l</span><span class="p">])</span>
            <span class="n">left_p_ls</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">vstack</span><span class="p">([</span><span class="n">left_ukf</span><span class="o">.</span><span class="n">state</span><span class="o">.</span><span class="n">p_l</span><span class="p">,</span> <span class="n">left_p_l</span><span class="p">])</span>
            <span class="n">right_p_ls</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">vstack</span><span class="p">([</span><span class="n">right_ukf</span><span class="o">.</span><span class="n">state</span><span class="o">.</span><span class="n">p_l</span><span class="p">,</span> <span class="n">right_p_l</span><span class="p">])</span>
            <span class="n">iekf_p_ls</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">vstack</span><span class="p">([</span><span class="n">iekf</span><span class="o">.</span><span class="n">state</span><span class="o">.</span><span class="n">p_l</span><span class="p">,</span> <span class="n">iekf_p_l</span><span class="p">])</span>
            <span class="n">ekf_p_ls</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">vstack</span><span class="p">([</span><span class="n">ekf</span><span class="o">.</span><span class="n">state</span><span class="o">.</span><span class="n">p_l</span><span class="p">,</span> <span class="n">ekf_p_l</span><span class="p">])</span>
            <span class="n">ukf</span><span class="o">.</span><span class="n">state</span><span class="o">.</span><span class="n">p_l</span> <span class="o">=</span> <span class="n">p_l</span>
            <span class="n">left_ukf</span><span class="o">.</span><span class="n">state</span><span class="o">.</span><span class="n">p_l</span> <span class="o">=</span> <span class="n">left_p_l</span>
            <span class="n">right_ukf</span><span class="o">.</span><span class="n">state</span><span class="o">.</span><span class="n">p_l</span> <span class="o">=</span> <span class="n">right_p_l</span>
            <span class="n">iekf</span><span class="o">.</span><span class="n">state</span><span class="o">.</span><span class="n">p_l</span> <span class="o">=</span> <span class="n">iekf_p_l</span>
            <span class="n">ekf</span><span class="o">.</span><span class="n">state</span><span class="o">.</span><span class="n">p_l</span> <span class="o">=</span> <span class="n">ekf_p_l</span>

            <span class="c1"># get Jacobian and then covariance</span>
            <span class="n">R_n</span> <span class="o">=</span> <span class="n">obs_std</span> <span class="o">**</span> <span class="mi">2</span> <span class="o">*</span> <span class="n">np</span><span class="o">.</span><span class="n">eye</span><span class="p">(</span><span class="mi">2</span><span class="p">)</span>
            <span class="n">ukf</span><span class="o">.</span><span class="n">aug</span><span class="p">(</span><span class="n">y_n</span><span class="p">[</span><span class="n">idx0</span><span class="p">,</span> <span class="p">:</span><span class="mi">2</span><span class="p">],</span> <span class="n">aug_idxs</span><span class="p">,</span> <span class="n">R</span><span class="p">)</span>
            <span class="n">left_ukf</span><span class="o">.</span><span class="n">aug</span><span class="p">(</span><span class="n">y_n</span><span class="p">[</span><span class="n">idx0</span><span class="p">,</span> <span class="p">:</span><span class="mi">2</span><span class="p">],</span> <span class="n">aug_idxs</span><span class="p">,</span> <span class="n">R</span><span class="p">)</span>
            <span class="n">right_ukf</span><span class="o">.</span><span class="n">aug</span><span class="p">(</span><span class="n">y_n</span><span class="p">[</span><span class="n">idx0</span><span class="p">,</span> <span class="p">:</span><span class="mi">2</span><span class="p">],</span> <span class="n">aug_idxs</span><span class="p">,</span> <span class="n">R</span><span class="p">)</span>
            <span class="n">iekf</span><span class="o">.</span><span class="n">aug</span><span class="p">(</span><span class="n">y_n</span><span class="p">[</span><span class="n">idx0</span><span class="p">,</span> <span class="p">:</span><span class="mi">2</span><span class="p">],</span> <span class="n">aug_idxs</span><span class="p">,</span> <span class="n">R</span><span class="p">)</span>
            <span class="n">ekf</span><span class="o">.</span><span class="n">aug</span><span class="p">(</span><span class="n">y_n</span><span class="p">[</span><span class="n">idx0</span><span class="p">,</span> <span class="p">:</span><span class="mi">2</span><span class="p">],</span> <span class="n">aug_idxs</span><span class="p">,</span> <span class="n">R</span><span class="p">)</span>
            <span class="n">ukf</span><span class="o">.</span><span class="n">state</span><span class="o">.</span><span class="n">p_l</span> <span class="o">=</span> <span class="n">p_ls</span>
            <span class="n">left_ukf</span><span class="o">.</span><span class="n">state</span><span class="o">.</span><span class="n">p_l</span> <span class="o">=</span> <span class="n">left_p_ls</span>
            <span class="n">right_ukf</span><span class="o">.</span><span class="n">state</span><span class="o">.</span><span class="n">p_l</span> <span class="o">=</span> <span class="n">right_p_ls</span>
            <span class="n">iekf</span><span class="o">.</span><span class="n">state</span><span class="o">.</span><span class="n">p_l</span> <span class="o">=</span> <span class="n">iekf_p_ls</span>
            <span class="n">ekf</span><span class="o">.</span><span class="n">state</span><span class="o">.</span><span class="n">p_l</span> <span class="o">=</span> <span class="n">ekf_p_ls</span>

        <span class="c1"># save estimates</span>
        <span class="n">ukf_states</span><span class="o">.</span><span class="n">append</span><span class="p">(</span><span class="n">ukf</span><span class="o">.</span><span class="n">state</span><span class="p">)</span>
        <span class="n">left_ukf_states</span><span class="o">.</span><span class="n">append</span><span class="p">(</span><span class="n">left_ukf</span><span class="o">.</span><span class="n">state</span><span class="p">)</span>
        <span class="n">right_ukf_states</span><span class="o">.</span><span class="n">append</span><span class="p">(</span><span class="n">right_ukf</span><span class="o">.</span><span class="n">state</span><span class="p">)</span>
        <span class="n">iekf_states</span><span class="o">.</span><span class="n">append</span><span class="p">(</span><span class="n">iekf</span><span class="o">.</span><span class="n">state</span><span class="p">)</span>
        <span class="n">ekf_states</span><span class="o">.</span><span class="n">append</span><span class="p">(</span><span class="n">ekf</span><span class="o">.</span><span class="n">state</span><span class="p">)</span>

        <span class="n">ukf_Ps</span><span class="o">.</span><span class="n">append</span><span class="p">(</span><span class="n">ukf</span><span class="o">.</span><span class="n">P</span><span class="p">)</span>
        <span class="n">left_ukf_Ps</span><span class="o">.</span><span class="n">append</span><span class="p">(</span><span class="n">left_ukf</span><span class="o">.</span><span class="n">P</span><span class="p">)</span>
        <span class="n">right_ukf_Ps</span><span class="o">.</span><span class="n">append</span><span class="p">(</span><span class="n">right_ukf</span><span class="o">.</span><span class="n">P</span><span class="p">)</span>
        <span class="n">iekf_Ps</span><span class="o">.</span><span class="n">append</span><span class="p">(</span><span class="n">iekf</span><span class="o">.</span><span class="n">P</span><span class="p">)</span>
        <span class="n">ekf_Ps</span><span class="o">.</span><span class="n">append</span><span class="p">(</span><span class="n">ekf</span><span class="o">.</span><span class="n">P</span><span class="p">)</span>

    <span class="c1"># get state trajectory</span>
    <span class="n">Rots</span><span class="p">,</span> <span class="n">ps</span> <span class="o">=</span> <span class="n">model</span><span class="o">.</span><span class="n">get_states</span><span class="p">(</span><span class="n">states</span><span class="p">,</span> <span class="n">model</span><span class="o">.</span><span class="n">N</span><span class="p">)</span>
    <span class="n">ukf_Rots</span><span class="p">,</span> <span class="n">ukf_ps</span> <span class="o">=</span> <span class="n">model</span><span class="o">.</span><span class="n">get_states</span><span class="p">(</span><span class="n">ukf_states</span><span class="p">,</span> <span class="n">model</span><span class="o">.</span><span class="n">N</span><span class="p">)</span>
    <span class="n">left_ukf_Rots</span><span class="p">,</span> <span class="n">left_ukf_ps</span> <span class="o">=</span> <span class="n">model</span><span class="o">.</span><span class="n">get_states</span><span class="p">(</span><span class="n">left_ukf_states</span><span class="p">,</span> <span class="n">model</span><span class="o">.</span><span class="n">N</span><span class="p">)</span>
    <span class="n">right_ukf_Rots</span><span class="p">,</span> <span class="n">right_ukf_ps</span> <span class="o">=</span> <span class="n">model</span><span class="o">.</span><span class="n">get_states</span><span class="p">(</span><span class="n">right_ukf_states</span><span class="p">,</span> <span class="n">model</span><span class="o">.</span><span class="n">N</span><span class="p">)</span>
    <span class="n">iekf_Rots</span><span class="p">,</span> <span class="n">iekf_ps</span> <span class="o">=</span> <span class="n">model</span><span class="o">.</span><span class="n">get_states</span><span class="p">(</span><span class="n">iekf_states</span><span class="p">,</span> <span class="n">model</span><span class="o">.</span><span class="n">N</span><span class="p">)</span>
    <span class="n">ekf_Rots</span><span class="p">,</span> <span class="n">ekf_ps</span> <span class="o">=</span> <span class="n">model</span><span class="o">.</span><span class="n">get_states</span><span class="p">(</span><span class="n">ekf_states</span><span class="p">,</span> <span class="n">model</span><span class="o">.</span><span class="n">N</span><span class="p">)</span>

    <span class="c1"># record errors</span>
    <span class="n">ukf_err</span><span class="p">[</span><span class="n">n_mc</span><span class="p">]</span> <span class="o">=</span> <span class="n">model</span><span class="o">.</span><span class="n">errors</span><span class="p">(</span><span class="n">Rots</span><span class="p">,</span> <span class="n">ukf_Rots</span><span class="p">,</span> <span class="n">ps</span><span class="p">,</span> <span class="n">ukf_ps</span><span class="p">)</span>
    <span class="n">left_ukf_err</span><span class="p">[</span><span class="n">n_mc</span><span class="p">]</span> <span class="o">=</span> <span class="n">model</span><span class="o">.</span><span class="n">errors</span><span class="p">(</span><span class="n">Rots</span><span class="p">,</span> <span class="n">left_ukf_Rots</span><span class="p">,</span> <span class="n">ps</span><span class="p">,</span> <span class="n">left_ukf_ps</span><span class="p">)</span>
    <span class="n">right_ukf_err</span><span class="p">[</span><span class="n">n_mc</span><span class="p">]</span> <span class="o">=</span> <span class="n">model</span><span class="o">.</span><span class="n">errors</span><span class="p">(</span><span class="n">Rots</span><span class="p">,</span> <span class="n">right_ukf_Rots</span><span class="p">,</span> <span class="n">ps</span><span class="p">,</span> <span class="n">right_ukf_ps</span><span class="p">)</span>
    <span class="n">iekf_err</span><span class="p">[</span><span class="n">n_mc</span><span class="p">]</span> <span class="o">=</span> <span class="n">model</span><span class="o">.</span><span class="n">errors</span><span class="p">(</span><span class="n">Rots</span><span class="p">,</span> <span class="n">iekf_Rots</span><span class="p">,</span> <span class="n">ps</span><span class="p">,</span> <span class="n">iekf_ps</span><span class="p">)</span>
    <span class="n">ekf_err</span><span class="p">[</span><span class="n">n_mc</span><span class="p">]</span> <span class="o">=</span> <span class="n">model</span><span class="o">.</span><span class="n">errors</span><span class="p">(</span><span class="n">Rots</span><span class="p">,</span> <span class="n">ekf_Rots</span><span class="p">,</span> <span class="n">ps</span><span class="p">,</span> <span class="n">ekf_ps</span><span class="p">)</span>

    <span class="c1"># record NEES</span>
    <span class="n">ukf_nees</span><span class="p">[</span><span class="n">n_mc</span><span class="p">]</span> <span class="o">=</span> <span class="n">model</span><span class="o">.</span><span class="n">nees</span><span class="p">(</span><span class="n">ukf_err</span><span class="p">[</span><span class="n">n_mc</span><span class="p">],</span> <span class="n">ukf_Ps</span><span class="p">,</span> <span class="n">ukf_Rots</span><span class="p">,</span> <span class="n">ukf_ps</span><span class="p">,</span> <span class="s1">&#39;STD&#39;</span><span class="p">)</span>
    <span class="n">left_ukf_nees</span><span class="p">[</span><span class="n">n_mc</span><span class="p">]</span> <span class="o">=</span> <span class="n">model</span><span class="o">.</span><span class="n">nees</span><span class="p">(</span><span class="n">left_ukf_err</span><span class="p">[</span><span class="n">n_mc</span><span class="p">],</span> <span class="n">left_ukf_Ps</span><span class="p">,</span>
                                     <span class="n">left_ukf_Rots</span><span class="p">,</span> <span class="n">left_ukf_ps</span><span class="p">,</span> <span class="s1">&#39;LEFT&#39;</span><span class="p">)</span>
    <span class="n">right_ukf_nees</span><span class="p">[</span><span class="n">n_mc</span><span class="p">]</span> <span class="o">=</span> <span class="n">model</span><span class="o">.</span><span class="n">nees</span><span class="p">(</span><span class="n">right_ukf_err</span><span class="p">[</span><span class="n">n_mc</span><span class="p">],</span> <span class="n">right_ukf_Ps</span><span class="p">,</span>
                                      <span class="n">right_ukf_Rots</span><span class="p">,</span> <span class="n">right_ukf_ps</span><span class="p">,</span> <span class="s1">&#39;RIGHT&#39;</span><span class="p">)</span>
    <span class="n">iekf_nees</span><span class="p">[</span><span class="n">n_mc</span><span class="p">]</span> <span class="o">=</span> <span class="n">model</span><span class="o">.</span><span class="n">nees</span><span class="p">(</span><span class="n">iekf_err</span><span class="p">[</span><span class="n">n_mc</span><span class="p">],</span> <span class="n">iekf_Ps</span><span class="p">,</span> <span class="n">iekf_Rots</span><span class="p">,</span> <span class="n">iekf_ps</span><span class="p">,</span>
                                 <span class="s1">&#39;RIGHT&#39;</span><span class="p">)</span>
    <span class="n">ekf_nees</span><span class="p">[</span><span class="n">n_mc</span><span class="p">]</span> <span class="o">=</span> <span class="n">model</span><span class="o">.</span><span class="n">nees</span><span class="p">(</span><span class="n">ekf_err</span><span class="p">[</span><span class="n">n_mc</span><span class="p">],</span> <span class="n">ekf_Ps</span><span class="p">,</span> <span class="n">ekf_Rots</span><span class="p">,</span> <span class="n">ekf_ps</span><span class="p">,</span> <span class="s1">&#39;STD&#39;</span><span class="p">)</span>
</pre></div>
</div>
<p class="sphx-glr-script-out">Out:</p>
<div class="sphx-glr-script-out highlight-none notranslate"><div class="highlight"><pre><span></span>Monte-Carlo iteration(s): 1/100
Monte-Carlo iteration(s): 2/100
Monte-Carlo iteration(s): 3/100
Monte-Carlo iteration(s): 4/100
Monte-Carlo iteration(s): 5/100
Monte-Carlo iteration(s): 6/100
Monte-Carlo iteration(s): 7/100
Monte-Carlo iteration(s): 8/100
Monte-Carlo iteration(s): 9/100
Monte-Carlo iteration(s): 10/100
Monte-Carlo iteration(s): 11/100
Monte-Carlo iteration(s): 12/100
Monte-Carlo iteration(s): 13/100
Monte-Carlo iteration(s): 14/100
Monte-Carlo iteration(s): 15/100
Monte-Carlo iteration(s): 16/100
Monte-Carlo iteration(s): 17/100
Monte-Carlo iteration(s): 18/100
Monte-Carlo iteration(s): 19/100
Monte-Carlo iteration(s): 20/100
Monte-Carlo iteration(s): 21/100
Monte-Carlo iteration(s): 22/100
Monte-Carlo iteration(s): 23/100
Monte-Carlo iteration(s): 24/100
Monte-Carlo iteration(s): 25/100
Monte-Carlo iteration(s): 26/100
Monte-Carlo iteration(s): 27/100
Monte-Carlo iteration(s): 28/100
Monte-Carlo iteration(s): 29/100
Monte-Carlo iteration(s): 30/100
Monte-Carlo iteration(s): 31/100
Monte-Carlo iteration(s): 32/100
Monte-Carlo iteration(s): 33/100
Monte-Carlo iteration(s): 34/100
Monte-Carlo iteration(s): 35/100
Monte-Carlo iteration(s): 36/100
Monte-Carlo iteration(s): 37/100
Monte-Carlo iteration(s): 38/100
Monte-Carlo iteration(s): 39/100
Monte-Carlo iteration(s): 40/100
Monte-Carlo iteration(s): 41/100
Monte-Carlo iteration(s): 42/100
Monte-Carlo iteration(s): 43/100
Monte-Carlo iteration(s): 44/100
Monte-Carlo iteration(s): 45/100
Monte-Carlo iteration(s): 46/100
Monte-Carlo iteration(s): 47/100
Monte-Carlo iteration(s): 48/100
Monte-Carlo iteration(s): 49/100
Monte-Carlo iteration(s): 50/100
Monte-Carlo iteration(s): 51/100
Monte-Carlo iteration(s): 52/100
Monte-Carlo iteration(s): 53/100
Monte-Carlo iteration(s): 54/100
Monte-Carlo iteration(s): 55/100
Monte-Carlo iteration(s): 56/100
Monte-Carlo iteration(s): 57/100
Monte-Carlo iteration(s): 58/100
Monte-Carlo iteration(s): 59/100
Monte-Carlo iteration(s): 60/100
Monte-Carlo iteration(s): 61/100
Monte-Carlo iteration(s): 62/100
Monte-Carlo iteration(s): 63/100
Monte-Carlo iteration(s): 64/100
Monte-Carlo iteration(s): 65/100
Monte-Carlo iteration(s): 66/100
Monte-Carlo iteration(s): 67/100
Monte-Carlo iteration(s): 68/100
Monte-Carlo iteration(s): 69/100
Monte-Carlo iteration(s): 70/100
Monte-Carlo iteration(s): 71/100
Monte-Carlo iteration(s): 72/100
Monte-Carlo iteration(s): 73/100
Monte-Carlo iteration(s): 74/100
Monte-Carlo iteration(s): 75/100
Monte-Carlo iteration(s): 76/100
Monte-Carlo iteration(s): 77/100
Monte-Carlo iteration(s): 78/100
Monte-Carlo iteration(s): 79/100
Monte-Carlo iteration(s): 80/100
Monte-Carlo iteration(s): 81/100
Monte-Carlo iteration(s): 82/100
Monte-Carlo iteration(s): 83/100
Monte-Carlo iteration(s): 84/100
Monte-Carlo iteration(s): 85/100
Monte-Carlo iteration(s): 86/100
Monte-Carlo iteration(s): 87/100
Monte-Carlo iteration(s): 88/100
Monte-Carlo iteration(s): 89/100
Monte-Carlo iteration(s): 90/100
Monte-Carlo iteration(s): 91/100
Monte-Carlo iteration(s): 92/100
Monte-Carlo iteration(s): 93/100
Monte-Carlo iteration(s): 94/100
Monte-Carlo iteration(s): 95/100
Monte-Carlo iteration(s): 96/100
Monte-Carlo iteration(s): 97/100
Monte-Carlo iteration(s): 98/100
Monte-Carlo iteration(s): 99/100
Monte-Carlo iteration(s): 100/100
</pre></div>
</div>
<div class="section" id="results">
<h3>Results<a class="headerlink" href="#results" title="Permalink to this headline">¶</a></h3>
<p>We first visualize the results for the last run, and then plot the orientation
and position errors averaged over Monte-Carlo.</p>
<div class="highlight-default notranslate"><div class="highlight"><pre><span></span><span class="c1"># get state</span>
<span class="n">Rots</span><span class="p">,</span> <span class="n">ps</span> <span class="o">=</span> <span class="n">model</span><span class="o">.</span><span class="n">get_states</span><span class="p">(</span><span class="n">states</span><span class="p">,</span> <span class="n">model</span><span class="o">.</span><span class="n">N</span><span class="p">)</span>
<span class="n">ukf_Rots</span><span class="p">,</span> <span class="n">ukf_ps</span> <span class="o">=</span> <span class="n">model</span><span class="o">.</span><span class="n">get_states</span><span class="p">(</span><span class="n">ukf_states</span><span class="p">,</span>  <span class="n">model</span><span class="o">.</span><span class="n">N</span><span class="p">)</span>
<span class="n">left_ukf_Rots</span><span class="p">,</span> <span class="n">left_ukf_ps</span> <span class="o">=</span> <span class="n">model</span><span class="o">.</span><span class="n">get_states</span><span class="p">(</span><span class="n">left_ukf_states</span><span class="p">,</span>  <span class="n">model</span><span class="o">.</span><span class="n">N</span><span class="p">)</span>
<span class="n">right_ukf_Rots</span><span class="p">,</span> <span class="n">right_ukf_ps</span> <span class="o">=</span> <span class="n">model</span><span class="o">.</span><span class="n">get_states</span><span class="p">(</span><span class="n">right_ukf_states</span><span class="p">,</span>  <span class="n">model</span><span class="o">.</span><span class="n">N</span><span class="p">)</span>
<span class="n">iekf_Rots</span><span class="p">,</span> <span class="n">iekf_ps</span> <span class="o">=</span> <span class="n">model</span><span class="o">.</span><span class="n">get_states</span><span class="p">(</span><span class="n">iekf_states</span><span class="p">,</span>  <span class="n">model</span><span class="o">.</span><span class="n">N</span><span class="p">)</span>
<span class="n">ekf_Rots</span><span class="p">,</span> <span class="n">ekf_ps</span> <span class="o">=</span> <span class="n">model</span><span class="o">.</span><span class="n">get_states</span><span class="p">(</span><span class="n">ekf_states</span><span class="p">,</span>  <span class="n">model</span><span class="o">.</span><span class="n">N</span><span class="p">)</span>

<span class="n">ukf_err</span><span class="p">,</span> <span class="n">left_ukf_err</span><span class="p">,</span> <span class="n">right_ukf_err</span><span class="p">,</span> <span class="n">iekf_err</span><span class="p">,</span> <span class="n">ekf_err</span> <span class="o">=</span> <span class="n">model</span><span class="o">.</span><span class="n">benchmark_plot</span><span class="p">(</span>
    <span class="n">ukf_err</span><span class="p">,</span> <span class="n">left_ukf_err</span><span class="p">,</span> <span class="n">right_ukf_err</span><span class="p">,</span> <span class="n">iekf_err</span><span class="p">,</span> <span class="n">ekf_err</span><span class="p">,</span> <span class="n">ps</span><span class="p">,</span> <span class="n">ukf_ps</span><span class="p">,</span>
    <span class="n">left_ukf_ps</span><span class="p">,</span> <span class="n">right_ukf_ps</span><span class="p">,</span> <span class="n">ekf_ps</span><span class="p">,</span> <span class="n">iekf_ps</span><span class="p">)</span>
</pre></div>
</div>
<ul class="sphx-glr-horizontal">
<li><img alt="../_images/sphx_glr_slam2d_001.png" class="sphx-glr-multi-img" src="../_images/sphx_glr_slam2d_001.png" />
</li>
<li><img alt="../_images/sphx_glr_slam2d_002.png" class="sphx-glr-multi-img" src="../_images/sphx_glr_slam2d_002.png" />
</li>
<li><img alt="../_images/sphx_glr_slam2d_003.png" class="sphx-glr-multi-img" src="../_images/sphx_glr_slam2d_003.png" />
</li>
</ul>
<p>We then compute the Root Mean Squared Error (RMSE) for each method both for
the orientation and the position.</p>
<div class="highlight-default notranslate"><div class="highlight"><pre><span></span><span class="n">model</span><span class="o">.</span><span class="n">benchmark_print</span><span class="p">(</span><span class="n">ukf_err</span><span class="p">,</span> <span class="n">left_ukf_err</span><span class="p">,</span> <span class="n">right_ukf_err</span><span class="p">,</span> <span class="n">iekf_err</span><span class="p">,</span> <span class="n">ekf_err</span><span class="p">)</span>
</pre></div>
</div>
<p class="sphx-glr-script-out">Out:</p>
<div class="sphx-glr-script-out highlight-none notranslate"><div class="highlight"><pre><span></span>Root Mean Square Error w.r.t. orientation (deg)
    -SO(2) x R^(2(1+L)) UKF: 3.01
    -left SE_{1+L}(2) UKF  : 3.30
    -right SE_{1+L}(2) UKF : 2.51
    -EKF                   : 3.02
    -IEKF                  : 2.51

Root Mean Square Error w.r.t. position (m)
    -SO(2) x R^(2(1+L)) UKF: 0.67
    -left SE_{1+L}(2) UKF  : 0.75
    -right SE_{1+L}(2) UKF : 0.55
    -EKF                   : 0.67
    -IEKF                  : 0.55
</pre></div>
</div>
<p>Right UKF and IEKF outperform the remaining filters.</p>
<p>We now compare the filters in term of consistency (NEES).</p>
<div class="highlight-default notranslate"><div class="highlight"><pre><span></span><span class="n">model</span><span class="o">.</span><span class="n">nees_print</span><span class="p">(</span><span class="n">ukf_nees</span><span class="p">,</span> <span class="n">left_ukf_nees</span><span class="p">,</span> <span class="n">right_ukf_nees</span><span class="p">,</span> <span class="n">iekf_nees</span><span class="p">,</span> <span class="n">ekf_nees</span><span class="p">)</span>
</pre></div>
</div>
<ul class="sphx-glr-horizontal">
<li><img alt="../_images/sphx_glr_slam2d_004.png" class="sphx-glr-multi-img" src="../_images/sphx_glr_slam2d_004.png" />
</li>
<li><img alt="../_images/sphx_glr_slam2d_005.png" class="sphx-glr-multi-img" src="../_images/sphx_glr_slam2d_005.png" />
</li>
</ul>
<p class="sphx-glr-script-out">Out:</p>
<div class="sphx-glr-script-out highlight-none notranslate"><div class="highlight"><pre><span></span>Normalized Estimation Error Squared (NEES) w.r.t. orientation
   -SO(2) x R^(2(1+L)) UKF:  2.93
   -left SE_{1+L}(2) UKF  :  10.38
   -right SE_{1+L}(2) UKF :  1.01
   -EKF                   :  2.88
   -IEKF                  :  1.01

Normalized Estimation Error Squared (NEES) w.r.t. position
   -SO(2) x R^(2(1+L)) UKF:  2.63
   -left SE_{1+L}(2) UKF  :  118.60
   -right SE_{1+L}(2) UKF :  1.09
   -EKF                   :  2.56
   -IEKF                  :  1.17
</pre></div>
</div>
<p>The right UKF and the IEKF obtain similar NEES and are the more consistent
filters, whereas the remaining filter have their NEES increasing.</p>
<p><strong>Which filter is the most accurate ?</strong> The <strong>right UKF</strong> and the <strong>IEKF</strong> are
the best both in term of accuracy and consistency.</p>
</div>
</div>
<div class="section" id="conclusion">
<h2>Conclusion<a class="headerlink" href="#conclusion" title="Permalink to this headline">¶</a></h2>
<p>This script compares different algorithms for 2D robot SLAM. The <strong>right UKF</strong>
and the <strong>IEKF</strong> are the more accurate filters. They are also consistent along
all the trajectory.</p>
<p>You can now:</p>
<ul class="simple">
<li><p>compare the filters in different scenarios. UKF and their (I)EKF
counterparts may obtain different results when noise is inflated.</p></li>
</ul>
<p class="sphx-glr-timing"><strong>Total running time of the script:</strong> ( 248 minutes  35.587 seconds)</p>
<div class="sphx-glr-footer class sphx-glr-footer-example docutils container" id="sphx-glr-download-auto-benchmark-slam2d-py">
<div class="sphx-glr-download docutils container">
<p><a class="reference download internal" download="" href="../_downloads/28e51f65839a935a69d9b19d748d8119/slam2d.py"><code class="xref download docutils literal notranslate"><span class="pre">Download</span> <span class="pre">Python</span> <span class="pre">source</span> <span class="pre">code:</span> <span class="pre">slam2d.py</span></code></a></p>
</div>
<div class="sphx-glr-download docutils container">
<p><a class="reference download internal" download="" href="../_downloads/a6fc81e359cbf1c3b5fe97a7ff0a0b40/slam2d.ipynb"><code class="xref download docutils literal notranslate"><span class="pre">Download</span> <span class="pre">Jupyter</span> <span class="pre">notebook:</span> <span class="pre">slam2d.ipynb</span></code></a></p>
</div>
</div>
<p class="sphx-glr-signature"><a class="reference external" href="https://sphinx-gallery.github.io">Gallery generated by Sphinx-Gallery</a></p>
</div>
</div>


           </div>
          </div>
          <footer>
  
    <div class="rst-footer-buttons" role="navigation" aria-label="footer navigation">
      
        <a href="../filter.html" class="btn btn-neutral float-right" title="Filters" accesskey="n">Next <span class="fa fa-arrow-circle-right"></span></a>
      
      
        <a href="inertial_navigation.html" class="btn btn-neutral" title="Navigation on Flat Earth - Benchmark" accesskey="p"><span class="fa fa-arrow-circle-left"></span> Previous</a>
      
    </div>
  

  <hr/>

  <div role="contentinfo">
    <p>
        &copy; Copyright 2019, Martin Brossard, Axel Barrau, Silvère Bonnabel.

    </p>
  </div>
  Built with <a href="http://sphinx-doc.org/">Sphinx</a> using a <a href="https://github.com/snide/sphinx_rtd_theme">theme</a> provided by <a href="https://readthedocs.org">Read the Docs</a>. 

</footer>

        </div>
      </div>

    </section>

  </div>
  


  

    <script type="text/javascript">
        var DOCUMENTATION_OPTIONS = {
            URL_ROOT:'../',
            VERSION:'alpha',
            COLLAPSE_INDEX:false,
            FILE_SUFFIX:'.html',
            HAS_SOURCE:  true
        };
    </script>
      <script type="text/javascript" src="../_static/jquery.js"></script>
      <script type="text/javascript" src="../_static/underscore.js"></script>
      <script type="text/javascript" src="../_static/doctools.js"></script>
      <script type="text/javascript" src="../_static/language_data.js"></script>
      <script type="text/javascript" src="https://cdnjs.cloudflare.com/ajax/libs/mathjax/2.7.5/latest.js?config=TeX-AMS-MML_HTMLorMML"></script>

  

  
  
    <script type="text/javascript" src="../_static/js/theme.js"></script>
  

  
  
  <script type="text/javascript">
      jQuery(function () {
          SphinxRtdTheme.StickyNav.enable();
      });
  </script>
   

</body>
</html>